#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1

// 引脚定义
#define POTENTIOMETER_PIN 33
#define X_PIN 34
#define Y_PIN 35

//油门引脚
#define SWITCH_PIN 4

#define OLED_SCL 17
#define OLED_SDA 16

const int gear_back=-1;
const int gear_advance=1;
const int gear_super_advance=2;

// 引脚定义
const int RPWM_PIN = 18;  // 连接BTS7960 RPWM
const int LPWM_PIN = 19;  // 连接BTS7960 LPWM
const int ENABLE_PIN = 5; // 连接R_EN和L_EN

// PWM参数
const int PWM_FREQ = 5000;     // PWM频率(Hz)
const int PWM_RESOLUTION = 8;  // 8位分辨率(0-255)
const int PWM_CHANNEL_R = 0;   // PWM通道0用于RPWM
const int PWM_CHANNEL_L = 1;   // PWM通道1用于LPWM

// 摇杆校准参数
int centerX = 1900;  // 中心点X值
int centerY = 1840;  // 中心点Y值
const int deadZone = 300; // 调整为更合理的死区值

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

void setup() {
  Serial.begin(115200);
  //初始化开关
  // 强制启用上拉（即使引脚有问题）
  pinMode(SWITCH_PIN, OUTPUT);
  digitalWrite(SWITCH_PIN, HIGH);  // 开启上拉
  pinMode(SWITCH_PIN, INPUT);      // 切换回输入模式但保持上拉
  
  // 初始化I2C
  Wire.begin(OLED_SDA, OLED_SCL);
  
  // 初始化OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    while(1); // 卡死
  }

  // 显示欢迎信息
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 10);
  display.println(F("Joystick Monitor"));
  display.println(F("Initialized..."));
  display.display();
  delay(1000); // 显示1秒

   // 初始化使能引脚
  pinMode(ENABLE_PIN, OUTPUT);
  digitalWrite(ENABLE_PIN, HIGH);  // 启用驱动芯片

  // 配置PWM通道
  ledcSetup(PWM_CHANNEL_R, PWM_FREQ, PWM_RESOLUTION);
  ledcSetup(PWM_CHANNEL_L, PWM_FREQ, PWM_RESOLUTION);
  
  // 绑定PWM通道到GPIO
  ledcAttachPin(RPWM_PIN, PWM_CHANNEL_R);
  ledcAttachPin(LPWM_PIN, PWM_CHANNEL_L);

  // 初始停止电机
  stopMotor();
}

void loop() {
  // 读取传感器数据
  int rawX = analogRead(X_PIN)-centerX;
  int rawY = analogRead(Y_PIN)-centerY;
  int sensorValue = analogRead(POTENTIOMETER_PIN);
  int gear = 0;
  // 计算电压值 (0-3.3V)
  float voltage = sensorValue * (3.3 / 4095.0);
  //开关定义
  int pinState = digitalRead(SWITCH_PIN);
  
  //防抖
  if(abs(rawX)<deadZone) rawX=0;
  if(abs(rawY)<deadZone) rawY=0;
  //挡位控制
  if(voltage<1.1) gear = gear_back;
  if(voltage>=1.1&voltage<2.2) gear = gear_advance;
  if(voltage>=2.2&voltage<=3.4) gear = gear_super_advance;

  // 清除并准备显示
  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(0, 0);
  
  // 显示原始摇杆数据
  display.print(F("Raw X:"));
  display.print(rawX);
  display.print(F(" Y:"));
  display.println(rawY);
  
  // 显示电位器数据
  display.print(F("gear:"));
  display.print(gear);
  switch(gear){
    case gear_back:display.println(",Back!");break;
    case gear_advance:display.println(",advance!");break;
    case gear_super_advance:display.println(",atention,superadvance!");break;
  }
  display.print("switch_state: ");
  display.println(pinState);
  display.print("voltage:");
  display.println(voltage);

  // 更新显示
  display.display();

  delay(100); // 刷新率100ms
}
// 设置电机速度(-255 ~ +255)
void setMotorSpeed(int speed) {
  if (speed > 0) {
    // 正转：RPWM输出PWM，LPWM输出低电平
    ledcWrite(PWM_CHANNEL_R, speed);
    ledcWrite(PWM_CHANNEL_L, 0);
  } else if (speed < 0) {
    // 反转：LPWM输出PWM，RPWM输出低电平
    ledcWrite(PWM_CHANNEL_R, 0);
    ledcWrite(PWM_CHANNEL_L, -speed);
  } else {
    // 停止
    stopMotor();
  }
}

// 停止电机
void stopMotor() {
  ledcWrite(PWM_CHANNEL_R, 0);
  ledcWrite(PWM_CHANNEL_L, 0);
}
